
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       log.c
  * @author     baiyang
  * @date       2023-2-23
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "logger.h"
#include "fms.h"
#include "define.h"

#include <logger/blog.h>
#include <uITC/uITC_msg.h>
#include <common/microbee.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
#pragma pack(1)
struct log_Control_Tuning {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    float    throttle_in;
    float    angle_boost;
    float    throttle_out;
    float    throttle_hover;
    float    desired_alt;
    float    inav_alt;
    int32_t  baro_alt;
    float    desired_rangefinder_alt;
    float    rangefinder_alt;
    float    terr_alt;
    int16_t  target_climb_rate;
    int16_t  climb_rate;
};

struct log_Data_Int16t {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t id;
    int16_t data_value;
};

struct log_Data_UInt16t {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t id;
    uint16_t data_value;
};

struct log_Data_Int32t {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t id;
    int32_t data_value;
};

struct log_Data_UInt32t {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t id;
    uint32_t data_value;
};

struct log_Data_Float {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t id;
    float data_value;
};

// guided position target logging
struct log_Guided_Position_Target {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t type;
    float pos_target_x;
    float pos_target_y;
    float pos_target_z;
    uint8_t terrain;
    float vel_target_x;
    float vel_target_y;
    float vel_target_z;
    float accel_target_x;
    float accel_target_y;
    float accel_target_z;
};

// guided attitude target logging
struct log_Guided_Attitude_Target {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t type;
    float roll;
    float pitch;
    float yaw;
    float roll_rate;
    float pitch_rate;
    float yaw_rate;
    float thrust;
    float climb_rate;
};

struct log_Vehicle_Gndeffect
{
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t armed;
    uint8_t maybe_landed;  //true if the vehicle might have landed (2. stage)
    uint8_t landed;        //true if vehicle is currently landed on the ground (3. stage)
    uint8_t in_ground_effect;  //indicates if from the perspective of the landing detector the vehicle might be in ground effect (baro). This flag will become true if the vehicle is not moving horizontally and is descending (crude assumption that user is landing).
    uint8_t at_rest;
};

#pragma pack()
/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
// type and unit information can be found in
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
// units and "Format characters" for field type information
static const struct LogStructure log_structure[] = {
    LOG_COMMON_STRUCTURES,

// @LoggerMessage: CTUN
// @Description: Control Tuning information
// @Field: TimeUS: Time since system startup
// @Field: ThI: throttle input
// @Field: ABst: angle boost
// @Field: ThO: throttle output
// @Field: ThH: calculated hover throttle
// @Field: DAlt: desired altitude
// @Field: Alt: achieved altitude
// @Field: BAlt: barometric altitude
// @Field: DSAlt: desired rangefinder altitude
// @Field: SAlt: achieved rangefinder altitude
// @Field: TAlt: terrain altitude
// @Field: DCRt: desired climb rate
// @Field: CRt: climb rate

// @LoggerMessage: D16
// @Description: Generic 16-bit-signed-integer storage
// @Field: TimeUS: Time since system startup
// @Field: Id: Data type identifier
// @Field: Value: Value

// @LoggerMessage: DU16
// @Description: Generic 16-bit-unsigned-integer storage
// @Field: TimeUS: Time since system startup
// @Field: Id: Data type identifier
// @Field: Value: Value

// @LoggerMessage: D32
// @Description: Generic 32-bit-signed-integer storage
// @Field: TimeUS: Time since system startup
// @Field: Id: Data type identifier
// @Field: Value: Value

// @LoggerMessage: DFLT
// @Description: Generic float storage
// @Field: TimeUS: Time since system startup
// @Field: Id: Data type identifier
// @Field: Value: Value

// @LoggerMessage: DU32
// @Description: Generic 32-bit-unsigned-integer storage
// @Field: TimeUS: Time since system startup
// @Field: Id: Data type identifier
// @Field: Value: Value

    { LOG_CONTROL_TUNING_MSG, sizeof(struct log_Control_Tuning),
      "CTUN", "Qffffffefffhh", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DSAlt,SAlt,TAlt,DCRt,CRt", "s----mmmmmmnn", "F----00B000BB" , true },
    { LOG_DATA_INT16_MSG, sizeof(struct log_Data_Int16t),         
      "D16",   "QBh",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_UINT16_MSG, sizeof(struct log_Data_UInt16t),         
      "DU16",  "QBH",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_INT32_MSG, sizeof(struct log_Data_Int32t),         
      "D32",   "QBi",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_UINT32_MSG, sizeof(struct log_Data_UInt32t),         
      "DU32",  "QBI",         "TimeUS,Id,Value", "s--", "F--" },
    { LOG_DATA_FLOAT_MSG, sizeof(struct log_Data_Float),         
      "DFLT",  "QBf",         "TimeUS,Id,Value", "s--", "F--" },

// @LoggerMessage: GUIP
// @Description: Guided mode position target information
// @Field: TimeUS: Time since system startup
// @Field: Type: Type of guided mode
// @Field: pX: Target position, X-Axis
// @Field: pY: Target position, Y-Axis
// @Field: pZ: Target position, Z-Axis
// @Field: Terrain: Target position, Z-Axis is alt above terrain
// @Field: vX: Target velocity, X-Axis
// @Field: vY: Target velocity, Y-Axis
// @Field: vZ: Target velocity, Z-Axis
// @Field: aX: Target acceleration, X-Axis
// @Field: aY: Target acceleration, Y-Axis
// @Field: aZ: Target acceleration, Z-Axis

    { LOG_GUIDED_POSITION_TARGET_MSG, sizeof(struct log_Guided_Position_Target),
      "GUIP",  "QBfffbffffff",    "TimeUS,Type,pX,pY,pZ,Terrain,vX,vY,vZ,aX,aY,aZ", "s-mmm-nnnooo", "F-BBB-BBBBBB" , true },

// @LoggerMessage: GUIA
// @Description: Guided mode attitude target information
// @Field: TimeUS: Time since system startup
// @Field: Type: Type of guided mode
// @Field: Roll: Target attitude, Roll
// @Field: Pitch: Target attitude, Pitch
// @Field: Yaw: Target attitude, Yaw
// @Field: RollRt: Roll rate
// @Field: PitchRt: Pitch rate
// @Field: YawRt: Yaw rate
// @Field: Thrust: Thrust 
// @Field: ClimbRt: Climb rate

    { LOG_GUIDED_ATTITUDE_TARGET_MSG, sizeof(struct log_Guided_Attitude_Target),
      "GUIA",  "QBffffffff",    "TimeUS,Type,Roll,Pitch,Yaw,RollRt,PitchRt,YawRt,Thrust,ClimbRt", "s-dddkkk-n", "F-000000-0" , true },

    { LOG_VEHICLE_GNDEFFECT, sizeof(struct log_Vehicle_Gndeffect),
      "GNDE",  "QBBBBB",    "TimeUS,Armed,MaybeL,Landed,InGndE,AtRest", "s-----", "F-----" , true },
};

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Write a control tuning packet
void aplog_write_control_tuning()
{
    // get terrain altitude
    float terr_alt = 0.0f;
#if 0
    if (!terrain.height_above_terrain(terr_alt, true)) {
        terr_alt = logger.quiet_nan();
    }
#endif
    float des_alt_m = 0.0f;
    int16_t target_climb_rate_cms = 0;
    if (!mode_has_manual_throttle(fms.flightmode)) {
        des_alt_m = posctrl_get_pos_target_z_cm(fms.pos_control) * 0.01f;
        target_climb_rate_cms = posctrl_get_vel_target_z_cms(fms.pos_control);
    }

    // get surface tracking alts
    float desired_rangefinder_alt;
    if (!surftrack_get_target_dist_for_logging(&fms.surface_tracking, &desired_rangefinder_alt)) {
        desired_rangefinder_alt = aplog_quiet_nan();
    }

    uitc_vehicle_status vehicle_status;
    uitc_vehicle_local_position local_position;

    (void)itc_copy_from_hub(ITC_ID(vehicle_local_position), &local_position);
    (void)itc_copy_from_hub(ITC_ID(vehicle_status), &vehicle_status);

    struct log_Control_Tuning pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
        .time_us             = time_micros64(),
        .throttle_in         = attctrl_get_throttle_in(fms.attitude_control),
        .angle_boost         = fms.attitude_control->_angle_boost,
        .throttle_out        = mb_motors_get_throttle(fms.motors),
        .throttle_hover      = mb_motors_get_throttle_hover(fms.motors),
        .desired_alt         = des_alt_m,
        .inav_alt            = ahrs_get_position_z_up_cm(fms.ahrs) * 0.01f,
        .baro_alt            = (-local_position.z - vehicle_status.arming_altitude_m)*100,
        .desired_rangefinder_alt = desired_rangefinder_alt,
        .rangefinder_alt     = surftrack_get_dist_for_logging(&fms.surface_tracking),
        .terr_alt            = terr_alt,
        .target_climb_rate   = target_climb_rate_cms,
        .climb_rate          = (int16_t)(ahrs_get_velocity_z_up_cms(fms.ahrs)) // float -> int16_t
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

// Write a control tuning packet
void aplog_write_vehicle_gndeffect()
{
    uitc_vehicle_gndeffect gndeffect;
    if (itc_copy_from_hub(ITC_ID(vehicle_gndeffect),  &gndeffect) == 0) {
        struct log_Vehicle_Gndeffect pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_VEHICLE_GNDEFFECT),
            .time_us             = time_micros64(),
            .armed               = gndeffect.armed,
            .maybe_landed        = gndeffect.maybe_landed,
            .landed              = gndeffect.landed,
            .in_ground_effect    = gndeffect.in_ground_effect,
            .at_rest             = gndeffect.at_rest,
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    };
}

// Write PIDS packets
void aplog_write_pids()
{
   if (aplog_should_log(MASK_LOG_PID)) {
        aplog_write_pid(LOG_PIDR_MSG, pid_ctrl_get_pid_info(attctrl_get_rate_roll_pid(fms.attitude_control)));
        aplog_write_pid(LOG_PIDP_MSG, pid_ctrl_get_pid_info(attctrl_get_rate_pitch_pid(fms.attitude_control)));
        aplog_write_pid(LOG_PIDY_MSG, pid_ctrl_get_pid_info(attctrl_get_rate_yaw_pid(fms.attitude_control)));
        aplog_write_pid(LOG_PIDA_MSG, pid_ctrl_get_pid_info(posctrl_get_accel_z_pid(fms.pos_control)));
        if (aplog_should_log(MASK_LOG_NTUN) && (mode_requires_GPS(fms.flightmode) || fms_landing_with_GPS())) {
            aplog_write_pid(LOG_PIDN_MSG, pid_2d_ctrl_get_pid_info_x(posctrl_get_vel_xy_pid(fms.pos_control)));
            aplog_write_pid(LOG_PIDE_MSG, pid_2d_ctrl_get_pid_info_y(posctrl_get_vel_xy_pid(fms.pos_control)));
        }
    }
}

// Write a Guided mode position target
// pos_target is lat, lon, alt OR offset from ekf origin in cm
// terrain should be 0 if pos_target.z is alt-above-ekf-origin, 1 if alt-above-terrain
// vel_target is cm/s
void aplog_write_guided_position_target(uint8_t target_type, const Vector3f_t *pos_target, bool terrain_alt, const Vector3f_t *vel_target, const Vector3f_t *accel_target)
{
    const struct log_Guided_Position_Target pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_GUIDED_POSITION_TARGET_MSG),
        .time_us         = time_micros64(),
        .type            = (uint8_t)target_type,
        .pos_target_x    = pos_target->x,
        .pos_target_y    = pos_target->y,
        .pos_target_z    = pos_target->z,
        .terrain         = terrain_alt,
        .vel_target_x    = vel_target->x,
        .vel_target_y    = vel_target->y,
        .vel_target_z    = vel_target->z,
        .accel_target_x  = accel_target->x,
        .accel_target_y  = accel_target->y,
        .accel_target_z  = accel_target->z
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

// Write a Guided mode attitude target
// roll, pitch and yaw are in radians
// ang_vel: angular velocity, [roll rate, pitch_rate, yaw_rate] in radians/sec
// thrust is between 0 to 1
// climb_rate is in (m/s)
void aplog_write_guided_attitude_target(uint8_t target_type, float roll, float pitch, float yaw, const Vector3f_t *ang_vel, float thrust, float climb_rate)
{
    const struct log_Guided_Attitude_Target pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_GUIDED_ATTITUDE_TARGET_MSG),
        .time_us         = time_micros64(),
        .type            = (uint8_t)target_type,
        .roll            = degrees(roll),       // rad to deg
        .pitch           = degrees(pitch),      // rad to deg
        .yaw             = degrees(yaw),        // rad to deg
        .roll_rate       = degrees(ang_vel->x),  // rad/s to deg/s
        .pitch_rate      = degrees(ang_vel->y),  // rad/s to deg/s
        .yaw_rate        = degrees(ang_vel->z),  // rad/s to deg/s
        .thrust          = thrust,
        .climb_rate      = climb_rate
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

// 注意不要在copter_log_init中调用
void aplog_write_vehicle_startup_messages()
{
    // only 200(?) bytes are guaranteed by AP_Logger
    aplog_write_messageF("MicrobeeCopter V%d.%d.%d-dev",FIRMWARE_VERSION);

    char frame_and_type_string[30];
    mb_motors_get_frame_and_type_string(fms.motors, frame_and_type_string, ARRAY_SIZE(frame_and_type_string));
    aplog_write_messageF("%s", frame_and_type_string);
    aplog_write_mode((uint8_t)mode_number(fms.flightmode), fms.control_mode_reason);
    //ahrs.Log_Write_Home_And_Origin();
    //gps.Write_AP_Logger_Log_Startup_messages();
}

int copter_log_init(void)
{
    aplog_init(&log_structure[0], ARRAY_SIZE(log_structure));
    return 0;
}
MB_INIT_COMPONENT_EXPORT(copter_log_init);
/*------------------------------------test------------------------------------*/


